#include "Vector4.h"
#include "Math.h"

namespace Core
{
	rtVector4 rtVector4::vec4_origin( 0.0f, 0.0f, 0.0f, 0.0f );

	inline rtVector4::rtVector4(void) 
	{
	}

	inline rtVector4::rtVector4(const float x, const float y, const float z, const float w) 
	{
		this->x = x;
		this->y = y;
		this->z = z;
		this->w = w;
	}

	inline void rtVector4::set(const float x, const float y, const float z, const float w) 
	{
		this->x = x;
		this->y = y;
		this->z = z;
		this->w = w;
	}

	inline void rtVector4::zero(void) 
	{
		x = y = z = w = 0.0f;
	}

	inline float rtVector4::operator [] (int index) const 
	{
		return (&x)[index];
	}

	inline float& rtVector4::operator [] (int index) 
	{
		return (&x)[index];
	}

	inline rtVector4 rtVector4::operator - () const 
	{
		return rtVector4(-x, -y, -z, -w);
	}

	inline rtVector4 rtVector4::operator - (const rtVector4& a) const 
	{
		return rtVector4(x - a.x, y - a.y, z - a.z, w - a.w);
	}

	inline float rtVector4::operator * (const rtVector4& a) const 
	{
		return x * a.x + y * a.y + z * a.z + w * a.w;
	}

	inline rtVector4 rtVector4::operator * (const float a) const 
	{
		return rtVector4( x * a, y * a, z * a, w * a );
	}

	inline rtVector4 rtVector4::operator / (const float a) const 
	{
		float inva = 1.0f / a;
		return rtVector4(x * inva, y * inva, z * inva, w * inva);
	}

	inline rtVector4 operator * (const float a, const rtVector4 b) 
	{
		return rtVector4(b.x * a, b.y * a, b.z * a, b.w * a);
	}

	inline rtVector4 rtVector4::operator + (const rtVector4& a) const 
	{
		return rtVector4(x + a.x, y + a.y, z + a.z, w + a.w);
	}

	inline rtVector4& rtVector4::operator += (const rtVector4& a) 
	{
		x += a.x;
		y += a.y;
		z += a.z;
		w += a.w;

		return *this;
	}

	inline rtVector4& rtVector4::operator /= (const rtVector4& a) 
	{
		x /= a.x;
		y /= a.y;
		z /= a.z;
		w /= a.w;

		return *this;
	}

	inline rtVector4& rtVector4::operator /= (const float a) 
	{
		float inva = 1.0f / a;
		x *= inva;
		y *= inva;
		z *= inva;
		w *= inva;

		return *this;
	}

	inline rtVector4& rtVector4::operator -= (const rtVector4 &a) 
	{
		x -= a.x;
		y -= a.y;
		z -= a.z;
		w -= a.w;

		return *this;
	}

	inline rtVector4 &rtVector4::operator *= (const float a) 
	{
		x *= a;
		y *= a;
		z *= a;
		w *= a;

		return *this;
	}

	inline bool rtVector4::compare(const rtVector4& a) const 
	{
		return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) && w == a.w );
	}

	inline bool rtVector4::compare(const rtVector4& a, const float epsilon) const 
	{
		if (rtMath::Fabs(x - a.x) > epsilon) 
		{
			return false;
		}

		if (rtMath::Fabs(y - a.y) > epsilon) 
		{
			return false;
		}

		if (rtMath::Fabs(z - a.z) > epsilon) 
		{
			return false;
		}

		if (rtMath::Fabs(w - a.w) > epsilon) 
		{
			return false;
		}

		return true;
	}

	inline bool rtVector4::operator == (const rtVector4& a) const 
	{
		return compare(a);
	}

	inline bool rtVector4::operator != (const rtVector4& a) const 
	{
		return !compare(a);
	}

	inline float rtVector4::length(void) const 
	{
		return (float)rtMath::Sqrt(x * x + y * y + z * z + w * w);
	}

	inline float rtVector4::lengthSqr(void) const 
	{
		return (x * x + y * y + z * z + w * w);
	}

	inline float rtVector4::normalize(void) 
	{
		float sqrLength, invLength;

		sqrLength = x * x + y * y + z * z + w * w;
		invLength = rtMath::InvSqrt(sqrLength);
		x *= invLength;
		y *= invLength;
		z *= invLength;
		w *= invLength;
		return invLength * sqrLength;
	}

	inline float rtVector4::normalizeFast(void) 
	{
		float sqrLength, invLength;

		sqrLength = x * x + y * y + z * z + w * w;
		invLength = rtMath::RSqrt(sqrLength);
		x *= invLength;
		y *= invLength;
		z *= invLength;
		w *= invLength;
		return invLength * sqrLength;
	}

	inline int rtVector4::getDimension(void) const 
	{
		return 4;
	}

	inline const rtVector2& rtVector4::toVec2(void) const 
	{
		return *reinterpret_cast<const rtVector2*>(this);
	}

	inline rtVector2& rtVector4::toVec2(void) 
	{
		return *reinterpret_cast<rtVector2*>(this);
	}

	inline const rtVector3& rtVector4::toVec3(void) const 
	{
		return *reinterpret_cast<const rtVector3*>(this);
	}

	inline rtVector3& rtVector4::toVec3( void ) 
	{
		return *reinterpret_cast<rtVector3 *>(this);
	}

	inline const float* rtVector4::toFloatPtr(void) const 
	{
		return &x;
	}

	inline float* rtVector4::toFloatPtr(void) 
	{
		return &x;
	}
	void rtVector4::lerp(const rtVector4& v1, const rtVector4& v2, const float l) 
	{
		if (l <= 0.0f) 
		{
			(*this) = v1;
		} 
		else if (l >= 1.0f) 
		{
			(*this) = v2;
		} 
		else 
		{
			(*this) = v1 + l * (v2 - v1);
		}
	}

}

